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ATTITUDE DETERMINATION USING VECTOR OBSERVATIONS: , * y 7^ / 

A FAST OPTIMAL MATRIX ALGORITHM 
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Abstract 

The attitude matrix minimizing Wahba's losslunction is computed directly by a method that 
is competitive with the fastest known algorithm for finding this optimal estimate. The method 
also provides an estimate of the attitude error covariance matrix. Analysis of the special case 
of two vector observations identifies those cases tor which the TRIAD or algebraic method 
minimizes Wahba's loss tunction. 


Introduction 

In 1965, Wahba posed the problem of finding the proper orthogonal matrix A that 
minimizes the non-negative loss function [1] 

L(A) = 2 ?^ I b |- Ar i I 2 ’ (1) 

where the unit vectors r f - are representations in a reference frame of the directions to some 
observed objects, the b, are the unit vector representations of the corresponding observations 
in the spacecraft body frame, the a; are positive weights, and n is the number of observations. 
The motivation for this loss function is that if the vectors are error-free and the true attitude 
matrix A true is assumed to be the same for all the measurements, then b ; is equal to A tru( x l 
for all i and the loss function is equal to zero for A equal to A true . 

Attitude determination algorithms based on minimizing this loss lunclion have been used 
for many years [2-9]. The original solutions to Wahba’s problem solved for the spacecralt 
attitude matrix directly [2-5], but most practical applications have been based on 
Davenport’s q-method [6-8], which solves for the quaternion representing the attitude 
matrix. In this paper, we present a new method that solves for the attitude matrix directly, as 
well as the covariance matrix, and which is competitive with the well known QUEST 
algorithm [9] in speed. Analysis of the special case of two observations serves to relate this 
method to the TRIAD or algebraic method [8, 9]. 


Statement of the problem 

Simple matrix manipulations transform the loss 1 unction into 

L(A) = A 0 - tr(A£ 7 ), 

where n 

*0 S Z , a i • 

t= 1 

fl T 1 

B = ? a t b/r/ , 

;=1 


( 2 ) 

(3) 

(4) 
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tr denotes the trace, and the superscript T denotes the matrix transpose. Thus Wahba's 
problem is equivalent to the problem of finding the proper orthogonal matrix A that maximizes 
the trace of the matrix product AB T . The weights are often chosen so that A 0 = 1, but this is 
not always the most convenient choice, as will be discussed below. 

This optimization problem has an interesting relation to a matrix norm. The Euclidean 
norm (also known as the Schur, Frobenius, or Hilbert-Schmidt norm) is defined for a general 
real matrix M by [10, 1 1] 


II M || 2 s ZMy 2 = lr(M M T ), 

where the sum is over all the matrix elements. The assumed orthogonality of A and 
properties of the trace give 

IU-£|| 2 = tr[(A - B)(A - B) 1 ] = tr / - 2\x(AB r ) +||fl|| 2 , 

where I is the 3x3 identity matrix. The orthogonal matrix A that maximizes M(AB T ) 
minimizes this norm, so Wahba’s problem is also equivalent to the problem of finding the 
pioper orthogonal matrix A that is closest to B in the Euclidean norm [12]. 

The matrix B can be shown to have the decomposition [13] 

B = U + diagfSj, S 2 , S 3 }V + T 

where U + and V + are proper orthogonal matrices; diag[...] denotes a matrix with the 
indicated elements on the main diagonal and zeros elsewhere; and *S’i» *S'2, an( l I So | , the 
singular values of B, obey the inequalities 

s,>s 2 > |s 3 |. 

The optimal altitude estimate is given in terms of these matrices by [13] 

A opt = U+V+ r ■ 


(5) 


(6) 


(7) 


( 8 ) 


(9) 


Equation (7) differs from the singular value decomposition (SVD) [10, 11] in that U + and V + 
are required to have positive determinant. In reference [13], S 3 was denoted by ds 3 , where 
d = ± 1 and ,v 3 > 0. 

The SVD provides a robust method for computing the matrices U + and V + , and thus the 
optimal attitude estimate, but it is not very etlicient [13]. The purpose of this paper is to 
present a more efficient method to estimate the attitude. 

Computation of the attitude matrix 

Noting that the adjoint of the transpose of B and the product BB T B can be written as 


and 


adj B 1 = U + diag[S 2 S 3 , S 3 S,, 5 { S 2 \ V + T , 
BB T B = U + diagf-S, 3 , S 2 3 , S 3 3 } V + T , 


( 10 ) 

(11) 
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( 12 ) 


it is a matter of simple algebra to see that 


A opt = [(K+ || B\\^)B + X adj B T - BB B ]/ £, 


where 


and 


II B || 2 = S, 2 + S 2 + S 3 , 

(13) 

are defined by 



(14) 

A = Sj + 52 + S g. 

(15) 

Cs (5 2 + 5 3 )(S 3 + 5 1 )(5 1 + 5 2 ). 

(16) 


The matrices in equation (12) can be computed without performing the singular value 
decomposition, but this equation is an improvement over equation (9) only because the scalar 
coefficients K A, and £ can also be computed without the SVD, as we will show below. 


Iterative computation of the scalar coefficients 

We first find expressions for the other scalar coefficients in terms of A. A little algebra 
shows that 


1 / T 2 

K= 2 (A 


iBih 


and 


£ = jcA- det B. 


(17) 

( 18 ) 


Let A(X) denote the expression for the attitude matrix given by equations (12), (17), and 
(18) as a function of A and B. This is equal to A opt if A is given by equation (15). Equations 
(7), (9), and (15) give 

A = tr {A opt B l ), ( 


so A can be computed as a solution of the equation 

A = tr[A(A)B 7 ] = tr[(tc+ ||fi || 2 )B£ 7 + A (det B)I - (BB T ) 2 ]/C- ( 2 °) 


Substitution of equations (17), (18), and the identity 

|| B|| 4 - tr[ (BB 1 ) 2 } =2 1| adj £ || 2 (2D 

lets us write this as 

0 = <2(A) = x: 2 - 2 A det B - II adj B || . (22) 

Since Kris a quadratic function of A, Q(X) is a quartic polynomial. It can be shown to be the 
same quartic that is used in QUEST, up to an irrelevant factor of one-fourth. Substitution of 
equation (7) into equation (22) gives the four roots of the quartic in terms of S v S 2 , and S 3 . 

We must use equation (17) for JC rather than equation (14) in this substitution, which gives 

4(9 (A) = a-S l -S 2 -S 3 ){X-S l +S 2 + S 3 )(A + S l -S 2 + S 3 )a + S l +S 2 -S 3 ). (23) 
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The roots ol this equation are all real, and they are the four eigenvalues of the K matrix 
in the q-method, as is well known [7, 9J. Equations (8) and (15) show that we require the 
maximum root, and that this root is distinct unless S 2 + S 3 = 0. When S 2 + S 3 = 0, the attitude 
solution is not unique, as is discussed in reference [13]; in the method introduced in this 
paper, this results in £= 0 and all the elements of A opl having the indefinite form 0/0. 

We now note from equations (2) and (19) that 

L(A 0 pt) = A^ — A > 0. (24) 

For small measurement errrors, the loss function should be close to zero, so the maximum 
root ol equation (22) should be close to A 0 [9]. Thus we can find A by Newton’s method, 
starting with this value. This defines a sequence of estimates of A by 

= ^/-l - Q ( 1 )/£?'( A/_j), / = 1,2 (25 ) 

Substitution ol equation (23) shows that this sequence would be monotonically decreasing 
with inlinite-precision arithmetic, but a computation with finite-precision arithmetic 
eventually finds a A,- > A,-_ j. At this point, the iterations are terminated and Aj_j is taken to 
be the desired root to lull computer precision. This iteration converges extremely rapidly in 
practice, except in the case that the maximum root of Q{X) is not unique. In that case the 
derivative in the denominator ol equation (25) goes to zero as the root is approached, so the 
computation is terminated and a warning is issued that the attitude is indeterminate. Halley's 
method [14] would give convergence in fewer iterations than Newton's method, but would 
require more computations per iteration, so it was not investigated further. 

It is important to carry out the computation of A to full machine precision, since otherwise 
the computed attitude matrix will not be orthogonal. Straightforward matrix computation gives 

A ( A) A 7 ( A) = / - Q ( A) (A 2 / - BB r )/C 2 . (26) 

This shows the orthogonality of the computed attitude matrix if A is a root of 0(A), and 
estimates the departure from orthogonality otherwise. 

Analytic computation of the scalar coefficients 


The scalar coellicients can also be computed as lunctions of the largest singular value 5i 
of B by 

K = 5,(5 2 + 5 3 ) + S 2 S 3 = S { (S 2 + S 3 ) + 5] _1 det B, (27) 

A = S, +(S 2 + S 3 ), (28) 

and 

C =(x'+5, 2 )(5 2 + 5 3 ), (29) 

where 

5 2 + 5 3 = {5i 2 l ll ac U & || 2 — (5’i _1 det £) 2 ] +25 1 “ , det B) m . (30) 

This form is chosen to avoid near-cancellations in near-singular cases. The largest singular 
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value is found as the positive square root ot the largest root of the cubic characteristic 

T 

equation of the matrix BB [7]: 

0 = (S|V - lr(BB , )(.S'| 2 ) 2 + lr|ad|(fi/i 7 ) |.V, 2 - dec( BB 1 ) 

= (S, 2 ) 3 - ||S || 2 (5| 2 ) 2 + || ad j B || 2 S, 2 - (del B) 2 . 

The largest root of this equation is given by [7, 15] 

S, 2 = || 5 || 2 + 2a l/2 cos[^ cos _, (a _ 3/2 /J )] }, 

cc= || 5 || 4 - 3 || adj B || 2 , 


where 

and 


p = || B || 6 - (9/2) || B || 2 1| adj B || 2 + (27/2)(det B) 2 . 


(31) 

(32) 

(33) 

(34) 


Equation (7) can be used to show that a > 0, with equality if and only if Sj = S 2 = I S 3 \ , in 
which case p= 0 also. Thus we have a complete analytic solution of Wahba's problem. 

Computation of the covariance matrix 

The quality of the attitude estimate is best expressed in terms of the covariance ol the 
three-component column vector <|) of attitude error angles in the spacecratt body trame. This 
parameterization gives the following relation between the estimated and true attitude 
matrices A and A 


true- 


A = {exp[(- <|))x] }A lrue = {/ - l<]> x] + ^ [<]) x ] 2 + ...}A true , 
where the matrix [u x] is defined lor a general three-component column vector u as 


(35) 


lux] = 


0 - M 3 «2 
t<3 0 -tq 

- U2 u i 0 


(36) 


This notation reflects the equality of the matrix product [ux]v and the cross product uxv. 

Shuster [16] has recast the Wahba problem as a maximum likelihood estimation problem 
[17], which leads to a very convenient method for computing the covariance matrix. Asymp- 
totically, as the amount of data becomes infinite, the covariance matrix tends to the inverse of 
the Fisher information matrix F, which is the expected value of the Hessian of the negative- 

log-likelihood function 7; ~ __ 

F jk =E[d 2 J/d<pjd(p k ]. (37) 

The distribution of the components of the i th measurement error vector perpendicular to the 
true vector are assumed to be Gaussian and axially symmetric about the true vector with 
variance Gj 2 per axis. Then the negative-log-likelihood lunction for this problem is [13, 16] 


/ = i£o i - 2 |b f -Ar i | 2 +.... 


(38) 
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where the omitted terms are independent oi attitude. For any positive Aq and with 


the weights 


9 n 

<W of V 

2 2 

a / = Vror 


are positive and satisfy equation (3). With this choice 


(39) 

(40) 


J = X 0 l o to ?U A ) + — , (41) 

which means that the solution to Wahba's problem is a maximum-likelihood estimate, since it 
minimizes the negative-log-likelihood function. Substituting equation (35) into equation (2) 
and using the identity 

[uxj |v x] = - (\ T u)I + vu 7 ( 42 ) 

gives, to second order, 

L{A) = A 0 -tr (A (rue B r ) + tr{[<> x]A true B 7 )+i tr{[(<^)7 ~W\A true B r ) 

= A 0 - tr (A lrue B J ) + tr{ [<(> xj A lrue B 1 } + \ty r [ir{A tme B T )1-A true B T ] <J). (43) 


Inserting this into equation (41) and then equation (37) gives the Fisher information matrix 
F-Aq Gf 0 f [tT(A (rue b' )I — ^(A lrue B + BA true )], (44) 

and, by matrix inversion, the covariance matrix 


P \)®tot~^ r ^true B ^ 2 ^ A true P + BA (ru J)} (45) 

The true attitude matrix is not known in a real attitude estimation problem, of course, so 
A opi must be used in place ot A lrue in computing the covariance. Making this replacement in 
equation (46) gives, with equation (19) and the symmetry of the matrix product A opt B T , 
which follows from equations (7) and (9), 

P = A 0 °toi'^ 1 - A opi B T )~ l = A 0 cr to , 2 ad j(A I -A opl S 7 )/det(A/ - A opt B T ). (46) 

Equation (46) is one of the forms for the covariance matrix given in Appendix B of [13], which 
is also the result obtained in [18], simplified to the case that only the attitude is estimated. 
The computation ot the matrix inverse can be avoided as follows [19]. Equations (7), (9), and 
(15) show that 

^ ~ A opt B = diag[S 2 + S3, S 3 + Sp S| + S 2 ]t/ + 7 . (47) 

The determinant of this matrix is given by equation (16) as 

det (A l-A opt B I ) = C (48) 

and its adjoint is 

adj(A/ -A opt B 1 ) = kI+BB T , (49) 

yielding the desired manifestly symmetric result 

p = a toh Kl + bb '*)/{;. ( 50 ) 
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We see that the covariance matrix is infinite when £ = 0, which agrees with the conditions 
for indeterminacy of the attitude solution discussed above. In the case of near-indeterminacy, 
the singular values are approximately « A, 5*2 ~ S 3 ~ 0 [13], which gives the covariance 

P - ^<y tot 2 U + diag[A 2 /C XT 1 , r l ]U + T . (51) 

A good criterion for terminating the iterative solution for A by equation (25) is 

eu) = 2f< o,o?- (52) 

since equation (51) predicts attitude estimation error standard deviations larger than 2 A/Aq 
radians when this inequality is satisfied. This error can only be small if A« Aq, in which case 
the attitude estimate is poor because the loss function is large. 

Normalization of the weights 

The results above are valid for any positive value of the parameter Aq, but only two 
choices are useful: 

Aq = 1 (normalized weights) (53) 

or 

Aq = <? to t 2 (unnormalized weights). (54) 

Past treatments of this problem have generally used normalized weights, which give a B 
matrix with elements of order unity. This is convenient in computations using fixed-point 
arithmetic, but floating-point arithmetic is an option on virtually all present-day computers. 
The normalized form may also be useful if the measurement weights are arbitrarily assigned. 

The unnormalized form is more natural if the weights are computed in terms of measure- 
ment variances, as in equation (40), since the unnormalized weights are just equal to the 
inverse variances. The unnormalized form also simplifies the computation of the covariance, 
as shown by equation (50), but this form can potentially lead to numerical problems. The 
elements of B are of order c to ~ 2 if the weights are not normalized, which means that 
|| adj B || 2 is of order <y ^ () ? 8 . Since can be of order 10 for highly accurate sensors, 

|| adj B i | 2 can be of order 1 () 48 , leading to exponent overflow in floating-point representations 
that do not provide an adequate exponent range. This is not a problem with double-precision 
arithmetic in conformity with ANSI/IEEE Standard 754-1985 for binary floating-point 
arithmetic [ 20 ], since this standard mandates eleven bits for the exponent, allowing 
representation of numbers as large as 10 3 * 38 . The Standard Apple Numerical Environment 
[21] and VAX G_FLOATING [22] double-precision arithmetic employ eleven-bit 
exponents, but VAX D_FLOATING double-precision arithmetic allots only eight bits for the 
exponent. This is the same as in IEEE-standard single-precision arithmetic, and allows 
representation of numbers only as large as 10 38 . Single-precision arithmetic would lead to 
exponent overflow problems for measurement variances less than about 10 , but 

double-precision arithmetic is certainly preferred in such cases. 
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Algorithm test - accuracy 


Two torms of the new algorithm, the form with the iterative solution for A (FOAM Fast 

Optimal Attitude Matrix), and the form with the analytic solution for 5, (SOMA Slower 

Optimal Matrix Algorithm), were compared with the SVD method [13] for minimizing 
Wahba's loss 1 unction. The three methods were implemented in double-precision FORTRAN 
and executed on a DEC VAX 8830 computer. FOAM and SOMA were implemented in 
FLOATING arithmetic with unnormalized weights. FOAM was also implemented with 
normalized weights in both G_FLOATING and D_FLOATING arithmetic, while the SVD 
method was implemented with unnormalized weights in D_FLOATING arithmetic. 


Four sets ot reference vectors were used for the tests: 


r, = [1, 0, of r 2 = [0, 1, Of, r 3 = [0, 0, if. 

(55) 

r, = [0.6, 0.8, of, r 2 = [0.8, - 0.6, of 

(56) 

r, = [1, 0, Of, r 2 = [1, 0.01, 0] 7 , r 3 = [1, 0, O.Olf, 

(57) 

1 = [1. o, of, r 2 = [0.96, 0.28, of , r 3 = [0.96, 0, 0.28] 7 . 

(58) 


Set (55) models three sensors with orthogonal boresights along the spacecraft body axes, 
while set (56) models two sensors with orthogonal boresights not along the body axes. 
Reterence vector set (57) is intended to model three star measurements in a single star 
sensor with a small lield-of-view. Set (58) models one sensor with its boresight along the 
body x-axis and two sensors with boresights 16.26 degrees off this axis. The observation 
vectors were computed as 


where 


1 true 


A true *7 + n /> 


(59) 

0.352 

0.864 

0.360 


0.864 

0.152 

0.480 

(60) 

0.360 -0.480 

0.800 



which has all non-zero matrix elements with exact decimal representations and is otherwise 
arbitrary, and n, is a vector of measurement errors. The tests were run both with n, = 0 and 
with measurement errors simulated by zero-mean Gaussian while noise on the components 
ot rtj . All the methods normalize the input observation and reference vectors; some 
elticiencies in the normalization process were found and applied to the three algorithms. 

The results of the accuracy tests are presented in Table 1. The reference vector sets are 
labeled REF. The standard deviations (in radians) in the table were used to compute the 
measurement weights and also the level ol measurement errors in the tests where these 
were simulated. Only two measurements were used in the tests in which only two standard 
deviations are given. The quantities presented in the table are the estimation error in radians 
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(computed with simulated measurement errors), 


EST= sin '(2 3/2 II A 


4 1 - A 

opt n true ™ true 


aJ, II), 


‘op/ II >■> 

the maximum computation error for all FOAM and SOMA variants (computed with = 0), 


A opt A true 


COMP- 

and the maximum orthogonality error for FOAM and SOMA, 

ORTH= \\A op( A 0 p t -I 


(62) 


(63) 


The estimation error was the same for all methods, to the accuracy of the computation errors. 
As expected, the very robust SVD method gives the smallest maximum orthogonality error 
(2.16 x 1(T 16 ) and computation errors (4.72 x 10“ 17 for cases 1 - 4, 1.63 x 10 for case 5, 
3.74 x 10 -15 for cases 6 - 1 1, and 2.10 x 10~ 9 for case 12). No significant differences were 
seen between FOAM and SOMA or between normalized and unnormalized weights. 
^FLOATING arithmetic was about one decimal digit more precise than G_FL0AT1NG 
arithmetic, as expected [22]; but this is not significant, since the computation errors are much 
less than the estimation errors in all cases with realistic noise. It is clear that cases with 
widely differing measurement accuracies furnish the greatest computational challenges. 

Algorithm test - speed 

The above methods were compared with Shuster's QUEST (QUatemion ESTimation) 
algorithm [9] for computational speed, since QUEST is the fastest previously known 
algorithm for solving Wahba's problem. In addition to the reference and observation vectors 

Table 1 

Accuracy Test Results. See text for explanation 


CASE REF 


a, 




EST 


COMP 


ORTH 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 
11 
12 


(55) 

(55) 

(55) 

(55) 

(56) 

(57) 
(57) 
(57) 

(57) 

(58) 
(58) 
(58) 


10 


-6 


10 


10 -6 

.01 

.01 


,-6 

| 

-6 


-6 


10 

10 


-6 

-6 

-6 


10 

.01 

.01 


10 


-6 

,-6 


10' 

.01 


10 

.01 

.01 

.01 

10" 6 

HT 6 

.01 

.01 

.01 

.01 

-6 


10 

.01 


1.38 x 10 


-6 


— 2.02 x 10“ 


1.39 x 10 


2.05 x 10 


r 2 

,-2 


4.61 x 10 
3.05 x 10' 
5.27 x 10 
3.05 x 10 


-16 


16 

-16 

-16 


— 1.12x10“ 


7.83 x 10“ 


-6 


2.51 x 10“ 
3.18 x 10 
0.186 
8.82 x 10 
1.72 x 10 


10 


10 
.01 
.01 

— 3.33x10“ 

— 3.48 x 10 


4.66 x 10 


-12 


-5 


-2 

-2 


7.84 x 10 
4.04 x 10 
5.70 x 10 


,-12 


-12 


-12 


1.12 x 10 

6.11 x 10 
1.01 x 10 

1.12 x 10 
2.73 x 10 
8.94 x 10 
1.54 x 10' 
7.50 x 10 
1.12 x 10' 


r 15 

r 16 

,-15 


-15 


-8 


,-12 


-11 

-12 

11 


-2 


1.49 x 10 
1.45 x 10 
3.01 x 10 


r 7 

- 7 

-7 


2.97 x 10“ 
2.87 x 10 
6.00 x 10“ 


-7 
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and the measurement standard deviations, QUEST requires the input of five control 
parameters, which were taken as QUIBBL = 0.1, FIBBL = 10 ~ 5 ,QUAOO- 10“ 8 , NEWT = 10, 
and IMETH = 1. The measured CPU times were effectively the same for normalized and 
unnormalized weights. They consist of a part that is independent of the number of 
observations processed and a part proportional to the number of observations: 


QUEST = 0-^4 + 0.09 n 

msec. 

(64) 

FOAM = ^’ 27 + 0,07 n 

msec. 

(65) 

SOMA = 0 36 + °' 07 n 

msec, 

(66) 

S VD = O ± 1 ) + 0-07 n 

msec. 

(67) 


The greater n-dependent time in QUEST as compared to the other algorithms is due to the 
method used to compute the covariance matrix in QUEST. The computation of A generally 
requires one or two iterations in QUEST and two to six iterations in FOAM, due to the need 
to iterate to convergence in the latter method, which accounts for the greater n-independent 
time in FOAM. The transcendental function calls in SOMA account for its longer running time 
compared to FOAM, which is definitely preferable to SOMA since it is faster and no less 
accurate. The range of times for the SVD method is related to the rank and conditioning of the 
B matrix. This method is significantly slower than all the other methods tested, as has been 
noted previously; but the SVD method may still find applications in nearly singular estimation 
problems. The exact CPU times will vary from case to case, and the time required for either 
FOAM or QUEST appears to be quite modest in comparison with other computations 
performed in spacecraft attitude determination. 

It should be pointed out that FOAM computes the attitude matrix directly, while QUEST 
computes an attitude quaternion. If an attitude matrix is required from QUEST, an additional 
step is required to compute it from the quaternion. This requires only multiplications and 
additions, though, and no transcendental function evaluations. If it is desired to compute a 
quaternion from FOAM, the standard method for extracting it from the attitude matrix can be 
used [23]. This requires the evaluation of one square root, but FOAM is faster than QUEST 
even with this addition. The principal advantage of FOAM over QUEST in practice is that it 
requires no control parameter input; its only inputs are the number of observations, the 
reference and observation vectors, and the measurement standard deviations. 

T wo-observation case 

In the special case of two observations, the rank of B is at most two, so det B = 0, which 
gives with equation (22) 

*■ = || adj B || , (68) 

A = (2k + || B || 2 ) 1/2 , (69) 

and 

C = kA. (70) 
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Both jcand A must be positive in order for A to be the largest root of 0(A). The explicit form 
for B as a function of the reference and observation vectors then yields 


adj B T = a x a 2 (b ,x b 2 ) (ij x r 2 ) T , 
k = a { a 2 1 bj x b 2 1 I r i x r 2 1 , 


(71) 

(72) 


(73) 


and y j 2 1/2 

A = [a x 2 + 2a l a 2 \\b l xb 2 \ | x r 2 1 + (b, b 2 )(r, r 2 )] + a 2 } • 

The attitude is indeterminate if either the two reference vectors or the two observation 
vectors are parallel or antiparallel. Thus we will assume that both 6 r , the angle between rj 
and r 2 , and 8 b , the angle between b, and 1^, are strictly greater than zero and strictly less 
than pi. Now set A 0 = a x + a 2 = 1 for the remainder of the discussion in this section, detine 

e= (6 b -6 r )/2, (74) 

and note that | e \ < nil. This allows the expression for A to be written more compactly as 

A = (1 - 4 a.tf 2 sin 2 £) 1/2 . (75) 


These expressions for A in the two-observation case are equivalent to equation (72) in [9]. 


It is convenient to write the optimal attitude estimate in terms 

of the orthonormal triads: 

r + s (r 2 + rj)/[2cos(0 r /2)], 

( 76a) 

r = (r 2 -r,)/[2sin(0 r /2)]. 

(76b) 

r + x r_ = ( 1 -j x r 2 )l \ ^ x r 2 1 , 

(76c) 

and 

b + s (b 2 + bj)/|2cos(0/,/2)]. 

(77a) 

b_ = (l^ - b,)/[2sin(0 /; /2)]. 

(77b) 

b + x b_ = (bj x b 2 )/ 1 bj x b 2 1 . 

(77c) 

Other orthogonal triads can be defined, but these preserve the maximum symmetry between 
the two measurements. The optimal attitude matrix expressed in terms ot these triads is 


Y j 1 

4 = (l - 4 a,fl 2 sin 2 e) _1/2 [cose (b + r + 7 + b_r_ 7 ) + (a, - a 2 )sine (b + r_ - b_r + )] 

+ (b + x b_) (r + x r_) 7 . ( 7 « 

It is interesting to note that a factor of a x a 2 in the denominator of equation (12) has cancelled 
an identical factor in the numerator. Thus the attitude estimate has a well-del ined limit as 
either a, or a 2 tends to zero, even though Wahba's loss function does not have a unique 
minimum in either limit. Another interesting property of the two-observation case is that the 
optimal estimate is independent of the weights when e = 0. Equations (24) and (75) with 
Aq = 1 show that the optimized loss function is zero if any ot a v a 2 , or e is zero. 
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We now investigate the conditions under which this optimal attitude estimate can be 
obtained by a generalization of the simpler TRIAD or algebraic method [8, 9], This is a 
well-known algorithm for computing an attitude matrix from two vector observations by 
forming orthonormal triads from the reference and observation vectors. One of vectors in the 
reterence triad is the normalized cross product of the two reference vectors, and the other two 
are orthonormal linear combinations of the two reference vectors. The most general form for 
the reference triad that we will consider is: 

r I =cosy/ r r + -sini// r r_ = [sind//, + 0 r l2)r x - sin(vr r - 0 r /2)r 2 ]/sin0 r , (79a) 

r n ee cost y r r_ + sinyz r r + = [cos(yr r - 9 r / 2)r 2 - cos(vr r + 0 r /2)rj]/sin0 r , (79b) 

r l x r D = r + x r - > (79c) 

where y/ r is some rotation angle in the plane spanned by r, and r 2 . The observation triad is 

bj =cost/^b + - sim/^ b_ = fsin(i//), + 9 b l 2)b, - sinf^ - /2)b 2 J/sin6| e> , (80a) 

b n =cos\f/ b b + &my/ b b + = [cos(i y b - 9 b /2)b 2 - cos(y/ b + 9 b /2)b l ]/sin9 b , (80b) 

bj x b n = b + x b_ , (80c) 

similarly. The angles t f/ r and y/ b are chosen to give more or less weight to the two vector 
measurements. The choice y r = \ j/ b = 0, for example, gives equal weight to the two 
measurements. The choice y/ r = 9 r f 2 and y/ b = Q b /2 gives 

r l = r l- (81a) 

r ll = (r 2 -cos0 r r,)/sin0 r , (gib) 

and similar relations lor bj and b n , with maximum weight on the first measurement. The 
choice y/ r = - 8 r / 2 and y/ b = - 6 b /2, on the other hand, gives 

r i = r 2- (82a) 

r II = - ( r i - cos0 r r 2 )/sint9 r , (82b) 

and similarly for bj and b,j, with maximum weight on the second measurement. The key point 
is that y/ r is some function of 6 r and the measurement weights, and y/ b is the same function of 
9 b and the weights. Note that this does not imply that y/ r = y b except in the case that e = 0. 
Often, the TRIAD method is understood to mean only the special cases of equations (81) or 
(82), rather than the generalized method specified by equations (79) and (80). 

The TRIAD attitude estimate is given by 

A TRIAD = : *>1 x *>iiJ [ r i ^ x r n ] r = b,r, r + bnr n r + (bj x bjj) (r r x r n ) r 

= cos( y/ b - i jf r ) (b + r + 7 + b r 7 ) + sin (\f/ b - y/ r ) (b + r_ 7 - b_r + r ) 

+ (b+xbj(r + xr_) r . ( 83 ) 

We now attempt to find angles y/ r and y/ b such that the TRIAD solution gives the optimal 
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attitude estimate of equation (78). We immediately find such angles in four special cases. 


1 ) if £ = o, then y/ r = < y b automatically, all TRIAD solutions are the same, and they all agree 
with the optimal estimate, which is independent of the weights in the loss function. 

2) If flj = a 2 = 1/2, the TRIAD solution with y/ r = y/ b = 0 and with vector triads given by 
equations (76) and (77) gives the optimal estimate. 

3) If aj = 1, a 2 = 0, the TRIAD solution with y/ r = 6 r l 2, y/ b = 9 b /2 and with triads as in 
equations (81) gives the optimal estimate. 

4) If = 0, a 2 = 1, the TRIAD solution with y r = - 6 r /2, y/ b = - 9 b l 2 and with triads as in 
equations (82) gives the optimal estimate. 

We will now show that the TRIAD solution does not minimize Wahba’s loss function except 
in these four special cases. Comparing equations (78) and (83) gives the following necessary 
condition for agreement of the TRIAD and optimal attitude estimates. 

tan(v^ - y/ r ) = (a { - a 2 ) tana < 84) 

Set 6 r = 0q, some arbitrarily chosen angle, and denote the corresponding value of y/ r by i/o* 
which is also a function of the observation weights. Then 


tan(y f b - l/p) = ( a i ~ o 2 )tan[(9 b - 0 Q )/ 2] = (a { -a 2 )?/ r ( 85 ) 

This equation must hold for any 6 r , with i//q and 6q regarded as fixed parameters, since y/ b is 
required to be a function of 6 b and the weights only, and not of 6 r . Now setting 8 b = 9 0 in 
equation (84) gives y/ b = i// 0 and 

tan(yz r - y/ () ) = (a ] - a 2 )fan[(0 r - 9 (] )/2\ = (a x -aj)T r » 

which must hold for any 9 b . In fact, equation (86) could have been written directly in analogy 
with equation (85), since if/ r is required to be the same function of 9 r and the measurement 
weights as y/ b is of 9 b and the weights. Now combining equations (85) and (86) with some 
elementary trigonometry gives 

2 

tan (y/ b - y r ) = tan[(v^- Vq) - = ( a i~ a 2 T r >^ 1 + ~ a 2 > x b T r^ 

= (tij -a 2 )ta n£(l + t b t r )/[l + (1 - 4a 1 a 2 )^' r r l- 

Equating the right sides of equations (84) and (87) gives, after some cancellations, the 
necessary condition 

4a l a 2 t b t r (a { - a 2 )fane= 0, ( 88 1 


which is satisfied in the four special cases discussed above. It is also satisfied if either z b or 
x is zero, but these conditions cannot be satisfied in general since 9 0 is an arbitrarily chosen 
angle. Thus the TRIAD method cannot find the optimal attitude minimizing Wahba’s loss 
function in the general case, but only in the special cases e = 0,a { = 0, a 2 = 0, and a x - a 2 . 
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Conclusions 


A new algorithm for minimizing Wahba’s loss function has been found, which solves for 
the optimal attitude matrix directly, without the intermediate computation of a quaternion or 
other parameterization of the attitude. The attitude quaternion can be computed from the 
attitude matrix, if desired; and the new method with iterative solution of the scalar 
coeflicients in the attitude matrix is at least as fast as existing methods even with this 
additional computation. The scalar coefficients used in computing the optimal attitude matrix 
are also used to compute the covariance of the attitude error angles. Since the attitude matrix 
is inherently nonsingular, there are no problems with special cases like 180 degree rotations, 
and no special procedures are needed to deal with such cases. The principal practical 
advantage of the new method over existing fast optimal attitude estimators is that it requires 
no control parameter input; its only inputs are the number of observations, the reference and 
observation vectors, and the measurement standard deviations. 

A closed-form solution for the optimal attitude matrix is presented for the special case of 
two observations. This solution is compared with the estimate produced by the well-known 
non-optimal method based on orthonormal triads formed from the observation and reference 
vectors. When the angle between the two reference vectors is equal to the angle between the 
two observation vectors, all triad choices give the optimal estimate, which is independent of 
the weights in the loss lunction. Except for this case, the optimal and triad-based attitude 
estimates agree only when the two vector measurements are given equal weights in the loss 
lunction or when the weight given to one vector measurement is negligible compared to the 
weight given to the other. 
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